import cv2 as cv
import numpy as np
import glob

#获取标定板 创建3维0矩阵
objp = np.zeros((6*4,3),np.float32)
#世界坐标到标定  所有的z坐标为0，赋值x，y
objp[:,:2] = np.mgrid[0:4,0:6].T.reshape(-1,2)
# print(objp)

#世界坐标点
obj_points = []
#图片坐标点
img_points = []
#图片文件路径
images = glob.glob(".\img\chessboards\chessboard01.jpg")
#遍历所有图片
for fname in images:
    img = cv.imread(fname)
    #灰度图像
    gray = cv.cvtColor(img,cv.COLOR_BGR2GRAY)
    #???
    size = gray.shape[::-1]
    ret = False
    #找到角点
    ret,corners = cv.findChessboardCorners(gray,(6,4))
    if ret:
        obj_points.append(objp)
        #亚像素集角点检测 octave
        # corners2 = cv.cornerSubPix(gray,corners,(5,5),(-1,-1),(cv.TERM_CRITERIA_MAX_ITER | cv.TERM_CRITERIA_EPS, 30, 0.001))
        # # 设置寻找亚像素角点的参数，采用的停止准则是最大循环次数30和最大误差容限0.001
        # if corners2.any():
        #     print("corners2")
        #     img_points.append(corners2)
        # else:
        #     print("corners")
        img_points.append(corners)
        #绘制角点
        cv.drawChessboardCorners(img,(6,4),corners,ret)
        cv.imshow('img',img)
        cv.waitKey(2000)

print(len(img_points))
cv.destroyAllWindows()

# 摄像机标定
ret,mtx,dist,rvecs,tvecs = cv.calibrateCamera(obj_points,img_points,size,None,None)
#
# print("ret: ",ret)
#内参矩阵
print("mtx:\n",mtx)
#畸变系数
print("dist:\n",dist)
# #旋转向量（外参)
# print("rvecs:\n",rvecs)
# #平移向量（外参）
# print("tvecs:\n",tvecs)

#畸变校正 - - 方法一
img = cv.imread(images[0])
h,w = img.shape[0:2]
#新的内参   使用不同的分辨率相同的相机标定
newcameramtx,roi = cv.getOptimalNewCameraMatrix(mtx,dist,(w,h),1,(w,h))
print(newcameramtx)
#根据图像内参校正变形图片
dst = cv.undistort(img,mtx,dist,None,newcameramtx)
x,y,w,h = roi
print(x,y,w,h)
dst1 = dst[y:y+h,x:x+w]
dst2 = cv.resize(dst1,(h*5,w*5),cv.INTER_LANCZOS4)
cv.imshow("img",dst2)
cv.waitKey(0)
cv.imwrite(".\img\calibrator4.jpg",dst2)

print(dst.shape)
print(dst1.shape)
print(dst2.shape)

#重映射 - - 方法二